/*
 * @Description: 
 * @Version: 0.0
 * @Autor: LQF
 * @Date: 2020-11-19 20:32:39
 * @LastEditors: LQF
 * @LastEditTime: 2020-11-20 16:37:37
 */
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <opencv2/opencv.hpp>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>

int main(int argc, char * argv[]) try
{

    ros::init(argc, argv, "image_publisher");
    ros::NodeHandle nh;
    image_transport::ImageTransport it(nh);
    image_transport::Publisher depth_pub = it.advertise("depth_image", 1);
    image_transport::Publisher color_pub = it.advertise("color_image", 1);    

    rs2::pipeline pipe;
    rs2::config cfg;
    cfg.enable_stream(RS2_STREAM_DEPTH, 1280, 720, RS2_FORMAT_Z16, 15);
    cfg.enable_stream(RS2_STREAM_COLOR, 1280, 720, RS2_FORMAT_RGB8, 15);
    pipe.start(cfg);
    rs2::align align_to_color(RS2_STREAM_COLOR);

    // rs2::decimation_filter dec_filter;  // Decimation - reduces depth frame density
    // dec_filter.set_option(RS2_OPTION_FILTER_MAGNITUDE, 8);
    rs2::hole_filling_filter hol_fillinf_filter; 
    hol_fillinf_filter.set_option(RS2_OPTION_HOLES_FILL, 0); // Use the value from the left neighbor pixel to fill the hole
    
    int index = 0;
    ros::Rate loop_rate(30);    
    while (nh.ok()) {

        // Block program until frames arrive
        rs2::frameset frameset = pipe.wait_for_frames();

        // Align all frames to color viewport
        frameset = align_to_color.process(frameset);
    
        // Try to get a frame of a depth image
        auto depth = frameset.get_depth_frame();
        auto color = frameset.get_color_frame();

        if (!depth || !color) 
            break;
        rs2::frame filtered = depth;
        // filtered = dec_filter.process(filtered);
        filtered = hol_fillinf_filter.process(filtered);   

        cv::Mat image(cv::Size(1280, 720), CV_8UC3, (void*)color.get_data(), cv::Mat::AUTO_STEP); 
        cv::Mat depthmat(cv::Size(1280, 720), CV_16U, (void*)filtered.get_data(), cv::Mat::AUTO_STEP); 

        std_msgs::Header h;
        h.stamp = ros::Time::now();
        sensor_msgs::ImagePtr color_msg = cv_bridge::CvImage(h, "rgb8", image).toImageMsg();
        sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(h, "16UC1", depthmat).toImageMsg();
        // ROS_INFO_STREAM("from realsense get rgb & depth images！");
        color_pub.publish(color_msg);
        depth_pub.publish(depth_msg);
        // ROS_INFO_STREAM("raw rgb & depth image data published！");

        ros::spinOnce(); 
        loop_rate.sleep();
    }

    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}
